/*
 * regler.cpp
 *
 *      Author: Jonas
 *      Letztes Update: 03.11.13 Christian | Code aufräumen
 *
 */

#include "regler.h"

int8_t motor_werte[4];

int8_t reglerWert_x = 0;
int8_t reglerWert_y = 0;
int16_t abweichung_x_summe = 0;
int16_t abweichung_y_summe = 0;
int16_t differenz_x_alt = 0;
int16_t differenz_y_alt = 0;
int16_t differenz_x = 0;
int16_t differenz_y = 0;

int32_t regler_x_temp = 0;
int32_t regler_y_temp = 0;

uint8_t anteil_p = 0;
uint8_t anteil_i = 0;
uint8_t anteil_d = 0;

uint8_t p_z = 0;
uint8_t i_z = 0;
uint8_t d_z = 0;


int16_t e_z_sum = 0;			// error sum
int32_t z_temp = 0;
int16_t e_z = 0; 				// error_z
int16_t e_z_old = 0;
/*
 * Hier werden die Werte für die Motoren aus dem aktuellen Winkel und dem Wunschwinkel kalkuliert
 */
void calcMotorValues(uint8_t throttle, int16_t winkel_x, int16_t winkel_y, int16_t v_Z ) {
	//Throttle limitieren
	if(throttle > THROTTLE_MAX) {

		throttle = THROTTLE_MAX;
	}

	if ( throttle > REGLER_MIN ) {

	//Winkel Integrieren
		differenz_x = winkel_x - angle_x;
		differenz_y = winkel_y - angle_y;
		abweichung_x_summe += differenz_x;
		abweichung_y_summe += differenz_y;

	// Begrenzung fuer Integrator Summe, wegen ueberlauf.

		if ( abweichung_x_summe > 30000 ) {

			abweichung_x_summe = 30000;
		} else if ( abweichung_x_summe < -30000 ) {

			abweichung_x_summe = -30000;
		}

		if ( abweichung_y_summe > 30000 ) {

			abweichung_y_summe = 30000;
		} else if ( abweichung_y_summe < -30000 ) {

			abweichung_y_summe = -30000;
		}


	//Neue Werte mittels PID-Regler ausrechnen
		regler_x_temp = anteil_p * differenz_x + anteil_i * ABTASTZEIT_IN_MS * abweichung_x_summe / 200 + anteil_d * (differenz_x - differenz_x_alt) * 10 / ABTASTZEIT_IN_MS ;

		regler_y_temp = anteil_p * differenz_y + anteil_i * ABTASTZEIT_IN_MS * abweichung_y_summe / 200 + anteil_d * (differenz_y - differenz_y_alt) * 10 / ABTASTZEIT_IN_MS ;

		reglerWert_x = (int8_t) ( regler_x_temp / 1000);
		reglerWert_y = (int8_t) (regler_y_temp / 1000);

	//Überprüfen, ob Reglerwerte Limit überschreiten, ggf. verkleinern

		// X-LIMITS

		if(reglerWert_x > REGLER_LIMIT) {

			reglerWert_x = REGLER_LIMIT;
		} else if ( reglerWert_x < 0 ) {

			if ( -1 * reglerWert_x >  REGLER_LIMIT) {

				reglerWert_x = -1 * REGLER_LIMIT;
			}
		}
	
		if (( throttle + reglerWert_x ) < REGLER_MIN) {

			reglerWert_x = REGLER_MIN - throttle;
		} else if (( throttle - reglerWert_x ) < REGLER_MIN) {

			reglerWert_x = throttle - REGLER_MIN;
		}

		// Y-LIMITS

		if(reglerWert_y > REGLER_LIMIT) {

			reglerWert_y = REGLER_LIMIT;
		} else if ( reglerWert_y < 0 ) {

			if ( -1 * reglerWert_y >  REGLER_LIMIT) {

				reglerWert_y = -1 * REGLER_LIMIT;
			}
		}

		if (( throttle + reglerWert_y ) < REGLER_MIN) {

			reglerWert_y = REGLER_MIN - throttle;
		} else if (( throttle - reglerWert_y ) < REGLER_MIN) {

			reglerWert_y = throttle - REGLER_MIN;
		}


	} else {
		reglerWert_x = 0;
		reglerWert_y = 0;
	}


	//Reglerwerte auf throttle addieren und als neue Motorwerte ablegen
	motor_werte[0] = throttle - reglerWert_x;
	motor_werte[1] = throttle + reglerWert_y;
	motor_werte[2] = throttle + reglerWert_x;
	motor_werte[3] = throttle - reglerWert_y;

	//Winkel zwischenspeichern für D-Anteil
	differenz_x_alt = differenz_x;
	differenz_y_alt = differenz_y;

	if ( throttle > REGLER_MIN ) {
		calc_Z( v_Z);
	}
}

void calc_Z(int16_t v) {

		if ( abs(angle_v_z) > VZ_MIN ) {

			e_z = v - angle_v_z;
			e_z_sum += e_z;

			// Begrenzung fuer Integrator Summe, wegen ueberlauf.

				if ( e_z_sum > 30000 ) {

					e_z_sum = 30000;
				} else if ( e_z_sum < -30000 ) {

					e_z_sum = -30000;
				}


			z_temp = p_z * e_z + i_z * ABTASTZEIT_IN_MS * e_z_sum / 200 + d_z * (e_z - e_z_old) * 10 / ABTASTZEIT_IN_MS ;

			z_temp = z_temp / 10;

			e_z_old = e_z;
			// LIMITS

			if ( z_temp > Z_LIMIT ) {
				z_temp = Z_LIMIT;
			} else if ( z_temp < 0 ) {
				if ( -1 * z_temp > Z_LIMIT ) {
					z_temp = -1 * Z_LIMIT;
				}
			}
			//TODO drehsinn
			motor_werte[0] -= z_temp;
			motor_werte[1] += z_temp;
			motor_werte[2] -= z_temp;
			motor_werte[3] += z_temp;


		}
}




void regler_init() {
	/* EEPROM Adressen
	 *
	 *	mode	| 0x00
	 *
	 * anteil_p | 0x01
	 * anteil_i | 0x02
	 * anteil_d | 0x03
	 *
	 * q_angle 	| 0x05
	 * q_gyro  	| 0x06
	 * r_angle 	| 0x07
	 * lowPass	| 0x08
	 *
	 * p_z 		| 0x09
	 * i_z 		| 0x0A
	 * d_z 		| 0x0B
	 *
	 */

	//Auslesen der Reglerwerte
	anteil_p = eeprom_read_byte( (uint8_t *) 0x01 );
	anteil_i = eeprom_read_byte( (uint8_t *) 0x02 );
	anteil_d = eeprom_read_byte( (uint8_t *) 0x03 );

	p_z = eeprom_read_byte( (uint8_t *) 0x09 );
	i_z = eeprom_read_byte( (uint8_t *) 0x0A );
	d_z = eeprom_read_byte( (uint8_t *) 0x0B );
}


void reglerwerte_update() {
	system_pause();
	// Alte Reglerwerte Senden


	send_buffer[0] = anteil_p;
	send_buffer[1] = anteil_i;
	send_buffer[2] = anteil_d;

	send_buffer[3] = p_z;
	send_buffer[4] = i_z;
	send_buffer[5] = d_z;

	UART_send(6);

//	while( !( UCSRA & (1<<UDRE)) );
//
//	UDR = anteil_p;
//	while( !( UCSRA & (1<<UDRE)) );
//
//	UDR = anteil_i;
//	while( !( UCSRA & (1<<UDRE)) );
//
//	UDR = anteil_d;
//	while( !( UCSRA & (1<<UDRE)) );

	// Neue Reglerwerte Empfangen

	while ( !(UCSRA & (1<<RXC)) );
	anteil_p = UDR;

	while ( !(UCSRA & (1<<RXC)) );
	anteil_i = UDR;

	while ( !(UCSRA & (1<<RXC)) );
	anteil_d = UDR;

	while ( !(UCSRA & (1<<RXC)) );
	p_z = UDR;

	while ( !(UCSRA & (1<<RXC)) );
	i_z = UDR;

	while ( !(UCSRA & (1<<RXC)) );
	d_z = UDR;

	// Daten in EEPROM schreiben

	eeprom_update_byte( (uint8_t *) 0x01, anteil_p );
	eeprom_update_byte( (uint8_t *) 0x02, anteil_i );
	eeprom_update_byte( (uint8_t *) 0x03, anteil_d );

	eeprom_update_byte( (uint8_t *) 0x09, p_z );
	eeprom_update_byte( (uint8_t *) 0x0A, i_z );
	eeprom_update_byte( (uint8_t *) 0x0B, d_z );

	system_reset();
}
